/*
* avr_test.c
*
* Created: 3/26/2011 22:35:52
*  Author: morpheus
*/ 

#include <stdlib.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/pgmspace.h>

#ifndef F_CPU
#define F_CPU 16000000UL
#endif
#include <util/delay.h>

#include "global.h"
#include "uart.h"
#include "atmega_adc.h"
#include "servo.h"

#define AREF 5

//uint16_t getPulseWidth();
//unsigned long pulseIn(uint8_t pin, uint8_t state, unsigned long timeout);

int main(void)
{
  uint8_t count = 0;
	char buffer0[20];
	//char buffer1[5];
	//char buffer3[5];
  //uint8_t angle = 0;
  BIT_up = true;// servo direction flag

  ServoState servoState;
  servoState.pin = PB2;
  servoState.angle = 90;

	//int value_t = 0;
	//uint8_t low, high;
  uint16_t distance0 = 0;
  uint16_t distance1 = 0;

  uart_init(BAUD_RATE);

  //_delay_ms(1000);
	//uart_puts("Initalize\n\r");

  initServo();
  attachServo(&servoState);

	//ADC
	initADC();
  readADC(0); // calibration of sensor
  _delay_ms(250);
  readADC(1); // calibration of sensor
  _delay_ms(250);
  


  while(1)
  {
  
    for(count = 0; count < 4; ++count)
    {
      distance0 += readADC(0) * AREF / 10;
      _delay_ms(1);
    }

    distance0 = distance0 / count;

    // filter crazy signals

	  //itoa(distance0,buffer0,10);
    
	  //itoa(distance1,buffer1,10);

	  //uart_puts("this is a test\n\r");

    //uart_puts("distance0 " );
    //uart_puts(buffer0);
    //uart_puts("\n\r");
    //uart_puts("distance1 " );
    //uart_puts(buffer1);
    //uart_puts("\n\r");

	  //itoa(servoState.angle ,buffer3,10);
    //uart_puts("angle " );


    sprintf(buffer0,"%d#%d#%d\n", distance0, distance1, servoState.angle);
    //uart_puts(buffer0);

    for(count = 0; count < 4; ++count)
    {
      distance1 += readADC(1) * AREF / 10;
      _delay_ms(1);
    }
    distance1 = distance1 / count;

    if( (distance0 > 15) && (servoState.angle > 70 && servoState.angle < 100) )
    {
      uart_puts("forward\n");
      uart_puts(buffer0);
      _delay_ms(250);
      // forward
	    //forward();
    }
    else if( (distance0 < 15) && (servoState.angle >= 70 && servoState.angle <= 90) )
    {
      // left
      uart_puts("left\n");
      uart_puts(buffer0);
      _delay_ms(250);
    }
    else if( (distance0 < 15) && (servoState.angle >= 90 && servoState.angle <= 100) )
    {
      // right
      uart_puts("right\n");
      uart_puts(buffer0);
      _delay_ms(250);
    }


    write_deg(&servoState);
    // Servo sweep direction
    if(BIT_up)
    {
      ++servoState.angle;
    }
    else
    {
      --servoState.angle;
    }
  
    if(servoState.angle == 180)
    {
      BIT_up = false;
    }
    else if(servoState.angle == 0)
    {
      BIT_up = true;
    }

	  //_delay_ms(50);
  }
}
